package Behaviors;

import lejos.nxt.Motor;
import lejos.robotics.navigation.Pose;
import lejos.robotics.subsumption.Behavior;
import sharedData.DebugItem;
import sharedData.MapData;
import sharedData.SharedConstants;
import NXTExplorer.SensorParameter;

/**
 * Detect behavior in subsumption architecture. Use the middle UltrasonicSensor
 * to detect objects in the detection range. Rotates the robot according to those readings.
 * 
 * @author Didzis Petkus, Danny Johansen, Liviu Berheci
 * 
 */
public class Detect implements Behavior {
	private boolean suppressed = false;

	private SensorParameter paramObj;

	private float dist;

	public Detect(SensorParameter params) {
		paramObj = params;
	}

	@Override
	public boolean takeControl() {
		dist = paramObj.getCenterUssr().getDistance();

		return dist <= SharedConstants.USSR_DETECTION_RANGE;
	}

	@Override
	public void action() {
		suppressed = false;
		// ExplorerBot.USSRMode = 0;

		sendDebug("Distance detected: " + dist);

		int angle = checkPremises();

		paramObj.getPilot().rotate(angle);

		handlePosition(paramObj.getPoseProvider().getPose());
	}

	/**
	 * Uses the middle UltrasonicSensor to determine turning angle. The turning
	 * angle is a random number representing turning direction(i.e. negative
	 * left, positive right). It will always be in range from -90 to + 90.
	 * 
	 * @return - turning angle
	 */
	private int checkPremises() {

		// look left.
		Motor.B.rotate(-75);
		float maxLeft = paramObj.getCenterUssr().getRange();

		// look right.
		Motor.B.rotate(150);
		float maxRight = paramObj.getCenterUssr().getRange();

		// look straight forward.
		Motor.B.rotate(-75);

		int random = (int) (Math.random() * 90) + 1;

		return maxLeft < maxRight ? -random : random;
	}

	/**
	 * Adds the robot position and sensor readings to the data queue based on
	 * given pose.
	 * 
	 * @param pose
	 *            - The given Pose object
	 */
	private void handlePosition(Pose pose) {
		if (pose == null)
			return;
		MapData data = new MapData(SharedConstants.DATA_TYPE_MAP_DATA,
				pose.getX(), pose.getY(), pose.getHeading(), paramObj
						.getLeftUssr().getRange(), paramObj.getRightUssr()
						.getRange(), paramObj.getCenterUssr().getRange());
		paramObj.getQueue().add(data);

	}

	@Override
	public void suppress() {
		suppressed = true;
	}

	/**
	 * Adds the debug information to the data queue with the given message.
	 * 
	 * @param msg
	 *            - the given message
	 */
	public void sendDebug(String msg) {
		paramObj.getQueue().add(
				new DebugItem(SharedConstants.DATA_TYPE_DEBUG, paramObj
						.getPoseProvider().getPose().getX(), paramObj
						.getPoseProvider().getPose().getY(), paramObj
						.getPoseProvider().getPose().getHeading(), msg));
	}

	// need to make sure we are as close to a perpendicular angle to the
	// wall we are going to follow.
	// if (angle < 0) {

	// // looking left.
	// Motor.B.rotate(-90);
	//
	// int averageFront = getAvgDistance(paramObj.getCenterUssr());
	//
	// int averageBack = getAvgDistance(paramObj.getLeftUssr()) - 3;
	//
	// int i = 0;
	// while (averageBack < averageFront) {
	//
	// i++;
	//
	// paramObj.getPilot().rotate(1);
	//
	// averageBack = getAvgDistance(paramObj.getLeftUssr());
	// averageFront = getAvgDistance(paramObj.getCenterUssr()) - 3;
	// }
	//
	// sendDebug("Corrections left: " + i);
	//
	// Motor.B.rotate(90);

	// handlePosition(paramObj.getPoseProvider().getPose());

	// ExplorerBot.USSRMode = -1;
	// }

	// else if (angle > 0) {
	// looking left.
	// Motor.B.rotate(90);
	//
	// int averageFront = getAvgDistance(paramObj.getCenterUssr());
	//
	// int averageBack = getAvgDistance(paramObj.getRightUssr()) - 3;
	//
	// int i = 0;
	// while (averageBack < averageFront) {
	//
	// i++;
	//
	// paramObj.getPilot().rotate(1);
	//
	// averageBack = getAvgDistance(paramObj.getRightUssr());
	// averageFront = getAvgDistance(paramObj.getCenterUssr()) - 3;
	// }
	//
	// sendDebug("Corrections right: " + i);
	//
	// Motor.B.rotate(-90);
	//
	// handlePosition(paramObj.getPoseProvider().getPose());

	// ExplorerBot.USSRMode = -2;
	// }

	// }

	// private int getAvgDistance(UltrasonicSensor sensor) {
	//
	// sensor.setMode(UltrasonicSensor.MODE_PING);
	//
	// sensor.ping();
	//
	// int[] readings = new int[8];
	//
	// int numOfReadings = sensor.getDistances(readings);
	//
	// int total = 0;
	// for(int i = 0 ; i < numOfReadings ; i++)
	// {
	// total =+ readings[i];
	// }
	// int average = total / numOfReadings;
	//
	// sensor.setMode(UltrasonicSensor.MODE_CONTINUOUS);
	//
	// return average;
	// }

}
